#include "pch.h"
#include "ShapeMatchingConstraint.h"
#include "PositionBasedDynamics.h"
#include "SimulationModel.h"

bool CShapeMatchingConstraint::initializeConstraint(CParticleData& vParticleData, const std::vector<int>& vParticleIndices, const std::vector<int>& vNumClusters, const std::vector<double>& vStiffness)
{
	for (int i = 0; i < m_NumOfParticles; i++)
	{
		m_ParticleIndices[i] = vParticleIndices[i];
		m_x0[i] = vParticleData.getPosition0(m_ParticleIndices[i]);
		m_w[i] = vParticleData.getInvMass(m_ParticleIndices[i]);
		m_numClusters[i] = vNumClusters[i];
		m_stiffness[i] = vStiffness[i];
	}

	bool res = CPositionBasedDynamics::init_ShapeMatchingConstraint(m_x0, m_w, m_NumOfParticles, m_restCm, m_invRestMat);
	return res;
}

bool CShapeMatchingConstraint::solveConstraintV(CParticleData& vParticleData)
{
	for (unsigned int i = 0; i < m_NumOfParticles; i++)
	{
		m_x[i] = vParticleData.getPosition(m_ParticleIndices[i]);
	}

	bool res = CPositionBasedDynamics::solve_ShapeMatchingConstraint(
		m_x0, m_x, m_w, m_NumOfParticles,
		m_restCm, m_invRestMat,
		m_stiffness,
		m_corr,
		m_RotationMatrix);

	if (res)
	{
		for (unsigned int i = 0; i < m_NumOfParticles; i++)
		{
			// Important: Divide position correction by the number of clusters 
			// which contain the vertex. 
			if (m_w[i] != 0.0)
			{
				vParticleData.getPosition(m_ParticleIndices[i]) += (1.0 / m_numClusters[i]) * m_corr[i];
			}
		}
	}
	return res;
}